clc
clear all
%%机器人模型导入
%urdf模型导入
% robot = importrobot("/home/maoli/Desktop/biped_v2/xacro/leg_v2_serial_left.urdf");
% robot = importrobot("/home/maoli/Desktop/biped_v2_calib/xacro/leg_v2_serial_left.urdf");
robot = importrobot("./leg_v3_left_serial(1).urdf");
% robot = importrobot("./leg_v3_right_serial(1).urdf");


% leg_ostrich_left_serial.urdf

robot.DataFormat='column';
robot.Gravity =[0 0 -9.81];%%



%%根据urdf模型确定刚体系统自由度
N_fixed = 0;
for i = 1:1:robot.NumBodies
    if( strcmp(robot.Bodies{1, i}.Joint.Type,'fixed') == true)
        N_fixed=N_fixed+1;
    end   
end
N_dof = robot.NumBodies - N_fixed;

%创建机器人刚体系统参数结构体：robot_para
robot_para.JointAxis = zeros(3,N_dof);
robot_para.JointToParentTransform = zeros(4,4,N_dof);
for i = 1:1:N_dof
    robot_para.JointAxis(:,i) = robot.Bodies{1,i+1}.Joint.JointAxis;
    robot_para.JointToParentTransform(:,:,i) = robot.Bodies{1,i+1}.Joint.JointToParentTransform;
end

%定义连杆的惯量 N_dof个自由度的连杆
I_c=zeros(3,3,N_dof);
I_o=zeros(3,3,N_dof);
d_oc=zeros(3,N_dof);
m=zeros(1,N_dof); %质量

%根据所导入的urdf模型读取各连杆的质量 惯量（相对于各刚体坐标系原点） 和质心位置向量（相对于各个刚体坐标系原点）
for i = 1:1:N_dof
    m(i) = robot.Bodies{1, i+1}.Mass; %由于urdf模型中，第一个刚体为固定基，因此第一个活动连杆的索引为2， 也是循环i+1的由来
    d_oc(:,i) = robot.Bodies{1, i+1}.CenterOfMass';
end

%模型得到的动力学参数集合 param =[mi micx micy micz i_xx i_xy i_xz i_yy i_yz i_zz]
param_model_matrix=zeros(10,N_dof);
for i=1:N_dof
    param_model_matrix(1,i)=m(i);
    param_model_matrix(2:4,i)=m(i)*d_oc(:,i);
    %%从urdf中导入各连杆相对于对应关节坐标系的转动惯量
    param_model_matrix(5,i)=robot.Bodies{1,i+1}.Inertia(1);% I_xx
    param_model_matrix(6,i)=robot.Bodies{1,i+1}.Inertia(6);% I_xy
    param_model_matrix(7,i)=robot.Bodies{1,i+1}.Inertia(5);% I_xz
    param_model_matrix(8,i)=robot.Bodies{1,i+1}.Inertia(2);% I_yy
    param_model_matrix(9,i)=robot.Bodies{1,i+1}.Inertia(4);% I_yz
    param_model_matrix(10,i)=robot.Bodies{1,i+1}.Inertia(3);% I_zz
end
Para_vec=zeros(N_dof*10,1);
for i = 1:1:N_dof
    Para_vec(10*(i-1)+1:10*i)=param_model_matrix(:,i);
end


%%计算惯量权重
omega_I = zeros(5*N_dof,1);
for i = 1:1:N_dof
    I_o(1,1,i) = param_model_matrix(5,i);
    I_o(1,2,i) = param_model_matrix(6,i);
    I_o(2,1,i) = param_model_matrix(6,i);
    I_o(1,3,i) = param_model_matrix(7,i);
    I_o(3,1,i) = param_model_matrix(7,i);
    I_o(2,2,i) = param_model_matrix(8,i);
    I_o(2,3,i) = param_model_matrix(9,i);
    I_o(3,2,i) = param_model_matrix(9,i);
    I_o(3,3,i) = param_model_matrix(10,i);
    
end


for i = 1:1:N_dof
    omega_I(10*(i-1)+1) = 1/Para_vec(10*(i-1)+1);
%     omega_I(10*(i-1)+2) = 1/(norm(Para_vec(10*(i-1)+2))+norm(Para_vec(10*(i-1)+3))+norm(Para_vec(10*(i-1)+4)));
%     omega_I(10*(i-1)+3) = 1/(norm(Para_vec(10*(i-1)+2))+norm(Para_vec(10*(i-1)+3))+norm(Para_vec(10*(i-1)+4)));
%     omega_I(10*(i-1)+4) = 1/(norm(Para_vec(10*(i-1)+2))+norm(Para_vec(10*(i-1)+3))+norm(Para_vec(10*(i-1)+4)));
    omega_I(10*(i-1)+2) = 1/(norm([Para_vec(10*(i-1)+2) Para_vec(10*(i-1)+3) Para_vec(10*(i-1)+4)]));
    omega_I(10*(i-1)+3) = 1/(norm([Para_vec(10*(i-1)+2) Para_vec(10*(i-1)+3) Para_vec(10*(i-1)+4)]));
    omega_I(10*(i-1)+4) = 1/(norm([Para_vec(10*(i-1)+2) Para_vec(10*(i-1)+3) Para_vec(10*(i-1)+4)]));
    
    [V,D] =eig(I_o(:,:,i));
%     trace(D)
    omega_I(10*(i-1)+5) = 1/trace(D);
    omega_I(10*(i-1)+6) = 1/trace(D);
    omega_I(10*(i-1)+7) = 1/trace(D);
    omega_I(10*(i-1)+8)  = 1/trace(D);
    omega_I(10*(i-1)+9)  = 1/trace(D);
    omega_I(10*(i-1)+10) = 1/trace(D);  
      
end
omega_I = omega_I/omega_I(1);
% omega_I(41:50) = 0.0001*omega_I(41:50);


%%数据读取
% data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation/real_leg_cali_fast.csv');
% data = table2array(data1);
% data_q = data(:,1:5)';
% data_qd = data(:,6:10)';
% data_tau = data(:,11:15)';

% data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation_v3/cali_legs_v3_real_2.csv');
data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation_v3/real_leg3_cali_4.csv');

data = table2array(data1);
data_q = data(:,1:5)';
data_qd = data(:,11:15)';
data_tau = data(:,21:25)';
% data_q = data(:,6:10)';
% data_qd = data(:,16:20)';
% data_tau = data(:,26:30)';

t = 0.002;%采样周期 0.002s
N_data =size(data_q,2);

% data_qdd = zeros(5,N_data - 1);
% for i = 1:N_data -1
%     data_qdd(:,i) = (data_qd(:,i+1) - data_qd(:,i))/t; 
% end

data_qd = zeros(5,N_data - 1);
data_qdd = zeros(5,N_data - 2);
for i = 1:N_data -1
    data_qd(:,i) = (data_q(:,i+1) - data_q(:,i))/t; 
end
for i = 1:N_data -2
    data_qdd(:,i) = (data_qd(:,i+1) - data_qd(:,i))/t; 
end


%%考虑实际数据充满噪声，对所有数据进行一阶滤波
% 一阶滤波公式y(n) = alpha*x(n) +(1-alpha)*y(n-1)

alpha = 0.05;
data_qdd_filted_ = data_qdd;
data_qd_filted_ = data_qd;
data_q_filted_ = data_q;
data_tau_filted_ =data_tau;
for i = 1:N_data -3
    data_qdd_filted_(:,i+1) = alpha*data_qdd_filted_(:,i+1) + (1-alpha)*data_qdd_filted_(:,i);
    data_qd_filted_(:,i+1) = alpha*data_qd_filted_(:,i+1) + (1-alpha)*data_qd_filted_(:,i);
    data_q_filted_(:,i+1) = alpha*data_q_filted_(:,i+1) + (1-alpha)*data_q_filted_(:,i);
    data_tau_filted_(:,i+1) = alpha*data_tau_filted_(:,i+1) + (1-alpha)*data_tau_filted_(:,i);

end

data_qdd_filted_(:,1:9898)=data_qdd_filted_(:,101:9998);
data_qd_filted_(:,1:9898)=data_qd_filted_(:,101:9998);
data_q_filted_(:,1:9898)=data_q_filted_(:,101:9998);
data_tau_filted_(:,1:9898)=data_tau_filted_(:,101:9998);
data_tau(:,1:9898)=data_tau(:,101:9998);



K = zeros(N_dof,N_dof*10,N_data - 1);
K_aug = zeros(N_dof,N_dof*12,N_data - 1);
for i = 1: N_data - 2
    [U,Uj] = Compute_LinearDynmatrix(data_q_filted_(:,i), data_qd_filted_(:,i), data_qdd_filted_(:,i),robot_para);
%     [U,Uj] = Compute_LinearDynmatrix(data_q(:,i), data_qd(:,i), data_qdd(:,i),robot_para);

    K(:,:,i) = Uj;
    K_aug(:,1:N_dof*10,i)=Uj;
    for j = 1:N_dof
%         K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd_filted_(j,i));  %Bc*sign(joint_velocity)
        gain_k=10;
        K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = tanh(gain_k * data_qd_filted_(j,i));  %Bc*tanh(k * joint_velocity) 摩擦力换向平滑
        K_aug(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd_filted_(j,i);  % B*joint_velocity        
%         K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd(j,i));  %Bc*sign(joint_velocity)
%         K_aug(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd(j,i);  % B*joint_velocity
    end
end
%%数据预处理及线性动力学矩阵计算完成
%%*****************************************%%
%                                           %
%%*****************************************%%

%%开始进行优化建模及求解
nb=size(data_tau,1);
n_data = 5000; %小于19998的数

cvx_begin sdp
    cvx_solver Mosek
%     cvx_precision high
    variable X(nb*12)
%     variable X_aug(nb*12)

    variable t1
    variable t2

    variable Bc(nb) %静摩擦力项
    variable B(nb)  %速度相关的粘滞摩擦力项
    expressions inertial_sdp_matrix(4,4,nb) % semidefinite  symmetric
    expressions C1(4,4) 
    expressions C2(4,4) 
    expressions C3(4,4) 
    expressions C4(4,4) 
    expressions C5(4,4) 
    expressions mj(nb)
    expressions hj(3,1,nb)
    expressions Ij(3,3,nb)
    expression error_norm1(n_data,1)
    
    joint_vel = data_qd;
%     tau = data_tau;%%使用原始力矩
    tau = data_tau_filted_;%%使用滤波后力矩

    X_ref = Para_vec;
    beta = 0.001;
    inertia_normal_vec = zeros(10*nb,1);
    normal_vec = zeros(10,1);
    normal_vec = [1 20 20 20 1000 1000 1000 1000 1000 1000]';
%     normal_vec = [1 1 1 1 1 1 1 1 1 1]';
    
    inertia_normal_vec = [normal_vec;
                            normal_vec;
                            normal_vec;
                            normal_vec;
                            normal_vec];
    
    
    K_square = zeros(size(K_aug,2),size(K_aug,2),n_data);
    K_sum =  zeros(size(K_aug,2),size(K_aug,2));
    BK_sum = zeros(1,size(K_aug,2));
    tau_square_sum =0;
    for i=1:n_data
        %%二次项累加系数
        K_square(:,:,i) = K_aug(:,:,i)'*K_aug(:,:,i);
        K_sum = K_sum +  K_square(:,:,i);
        %%常数项平方系数 sum(tau.T*K)=b.T*G
        BK_sum =  BK_sum +tau(:,i)'* K_aug(:,:,i);
        tau_square_sum = tau_square_sum + tau(:,i)'*tau(:,i);
    end
    K_sum = K_sum;% + 0.0001*eye(50); %%为了G能够求逆矩阵
    [Vk,Dk] = eig(K_sum);
    G = Vk*sqrt(Dk)*inv(Vk);
    G = real(G);  %% sum(K) = K_sum = G*G = G.transpose *G
%     G_inv = pinv(G);

    %% sum(tau.T*K)=b.T*G ;  b.T = sum(tau.T*K)*G^-1
    b = -BK_sum*pinv(G);
    b=b';
    %%从而求得||GX+b||^2 -bT*b + sum(tau.T*tau) 中的二次型系数G 和 b
    %%因此，将目标函数 sum(||YX-tau||^2) 转化为 ||GX+b||^2 -bT*b + sum(tau.T*tau)
    
    
    
%     for i=1:n_data
% %          error_norm1(i) = norm(K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i),2); 
% %          error_norm1(i) = (K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i))'*(K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i));    
%          error_norm1(i) = (K_aug(:,:,i)*X -tau(:,i))'*(K_aug(:,:,i)*X-tau(:,i));
%     end
    
%     error_norm1 = error_norm1/n_data;
%     minimize sum(error_norm1)/n_data + beta*norm((X-X_ref))
%     minimize ((G*X+b)'*(G*X+b)-b'*b + tau_square_sum + 5000*beta*norm((X(1:50,1)-X_ref).*inertia_normal_vec,2))
%     minimize (t1 + 5000*beta*norm((X(1:50,1)-X_ref).*inertia_normal_vec,2))
    minimize (t1 + 500*t2)

    for j = 1:nb
        mj(j) = X((j-1)*10+1);
        hj(:,:,j) = X((j-1)*10+2:(j-1)*10+4);
        Ij(:,:,j) = [X((j-1)*10+5) X((j-1)*10+6) X((j-1)*10+7);
                    X((j-1)*10+6) X((j-1)*10+8) X((j-1)*10+9);
                    X((j-1)*10+7) X((j-1)*10+9) X((j-1)*10+10)];
       
        inertial_sdp_matrix(4,4,j) = X((j-1)*10+1);
        inertial_sdp_matrix(1:3,1:3,j) = 0.5*trace(Ij(:,:,j))*eye(3) - Ij(:,:,j);
        inertial_sdp_matrix(4,1:3,j) =  X((j-1)*10+2:(j-1)*10+4)';
        inertial_sdp_matrix(1:3,4,j) =  X((j-1)*10+2:(j-1)*10+4);
    end
    X(51)>=0;
    X(52)>=0;
    X(53)>=0;
    X(54)>=0;
    X(55)>=0;
    X(56)>=0;
    X(57)>=0;
    X(58)>=0;
    X(59)>=0;
    X(60)>=0;
    
    C1 = inertial_sdp_matrix(:,:,1);
    C2 = inertial_sdp_matrix(:,:,2);
    C3 = inertial_sdp_matrix(:,:,3);
    C4 = inertial_sdp_matrix(:,:,4);
    C5 = inertial_sdp_matrix(:,:,5);
    subject to
        (G*X+b)'*(G*X+b) - b'*b + tau_square_sum < t1;
        ((X(1:50,1)-X_ref)'.*omega_I')*((X(1:50,1)-X_ref).*omega_I)<t2;
        C1 == semidefinite(4);
        C2 == semidefinite(4);
        C3 == semidefinite(4);
        C4 == semidefinite(4);
        C5 == semidefinite(4);%%此处约束未适用于任意自由度的urdf模型  需要修改

cvx_end

tau_est = zeros(nb,n_data);
tau_idel_X =zeros(nb,n_data);
for i = 1:1:n_data
%     tau_est(:,i) = K_aug(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i);
    tau_est(:,i) = K_aug(:,:,i)*X ;
    tau_idel_X(:,i) = K_aug(:,:,i)*[X_ref;[0 0 0 0 0 0 0 0 0 0]'] ;
end

for i =1:nb
    figure (i)
    T = t:t:t*n_data;
    plot(T,data_tau(i,1:n_data),'-.b')
    hold on
    plot(T,tau_est(i,1:n_data),'r')
    hold on
%     plot(T,tau_idel_X(i,1:n_data),'g')
%     hold on
%     plot(T,0.01*data_qdd(i,1:n_data),'--g')
    xlabel('t(s) 蓝色为测量值 红色为估计值 绿色是理论参数估计值' );
    ylabel('joint torque(Nm)')
end

